/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 *
 */
#include <stdint.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <inv_mpu.h>
#include <mpu6xxx.h>

#define MPU_READ_THREAD_NAME        "mpu_read"
#define MPU_READ_THREAD_STACK       4096
#define MPU_READ_THREAD_PRIORITY    20
#define MPU_READ_THREAD_TICK        10

struct mpu6xxx_device *mpu_dev;
static void mpu_read_thread(void *parameter)
{
    /* Initialize the mpu6xxx device */
    mpu_dev = mpu6xxx_init("i2c1", RT_NULL);
    while (mpu_dmp_init() != RT_EOK) {
        rt_kprintf("Initialize mpu6050 dmp failed!\n");
        rt_kprintf("Declare varible to save the result of the function mpu_dmp_init to view error code!\n");
        rt_thread_mdelay(1000);
    }

    /* store the ol angles */
    static float pitch[20], roll[20], yaw[20];
    static rt_int32_t pitch_100 = 0, roll_100 = 0, yaw_100 = 0;
    static rt_uint8_t cnt = 0;

    while (1) {
        for (cnt = 0; cnt < 20; cnt ++) {
            mpu_dmp_get_data(&pitch[cnt], &roll[cnt], &yaw[cnt]);
            rt_thread_mdelay(1);
        }
        rt_thread_mdelay(10);

        pitch_100 = 0;
        roll_100 = 0;
        yaw_100 = 0;
        for (cnt = 0; cnt < 20; cnt ++) {
            pitch_100 += pitch[cnt];
            roll_100 += roll[cnt];
            yaw_100 += yaw[cnt];
        }

        /* if the value of angle is larger than zero, send a letter P, otherwise send a letter N.*/
        if (pitch_100 < 0) {
            rt_kprintf("N%.5d", -1 * pitch_100 * 5);
        } else {
            rt_kprintf("P%.5d", pitch_100 * 5);
        }
        if (roll_100 < 0) {
            rt_kprintf("N%.5d", -1 * roll_100 * 5);
        } else {
            rt_kprintf("P%.5d", roll_100 * 5);
        }
        if (yaw_100 < 0) {
            rt_kprintf("N%.5d\n", -1 * yaw_100 * 5);
        } else {
            rt_kprintf("P%.5d\n", yaw_100 * 5);
        }
    }
}

int mpu_sample(void)
{
    rt_thread_t tid = rt_thread_create(MPU_READ_THREAD_NAME, mpu_read_thread, RT_NULL,
            MPU_READ_THREAD_STACK, MPU_READ_THREAD_PRIORITY, MPU_READ_THREAD_TICK);
    if (tid != RT_NULL) {
        rt_thread_startup(tid);
    } else {
        rt_kprintf("Create mpu6xxx read thread failed");
        return -RT_ERROR;
    }

    return RT_EOK;
}
MSH_CMD_EXPORT(mpu_sample, mpu read thread);

static void led_thread(void *parameter)
{
    /* Led blink */
    rt_pin_mode(67, PIN_MODE_OUTPUT);

    while (1) {
        rt_pin_write(67, PIN_LOW);
        rt_thread_mdelay(1000);
        rt_pin_write(67, PIN_HIGH);
        rt_thread_mdelay(1000);
    }
}

int led_sample(void)
{
    rt_thread_t tid = rt_thread_create("led", led_thread, RT_NULL, 1024, 9, 10);
    if (tid != RT_NULL) {
        rt_thread_startup(tid);
    } else {
        rt_kprintf("Create led blink thread failed");
        return -RT_ERROR;
    }

    return RT_EOK;
}
MSH_CMD_EXPORT(led_sample, led blink sample);

int main(void)
{
    /* Should be empty, any program will not be executed there */
}

